//
// Created by luo on 2022-10-16.
//

#ifndef ARMOR_DETECT_ANGLESOLVE_H
#define ARMOR_DETECT_ANGLESOLVE_H

#pragma once

#include <opencv2/opencv.hpp>
#include <iostream>
#include "Armor_detect.h"
//#include"../buffdistinguish/buff.h"

using namespace std;
using namespace cv;

//PNP解算部分
//摄像头中心距离云台转轴中心的固定偏移信息
#define X 0
#define Y 10
#define Z 125
#define PI 3.1415926

//云台转动轴限位最大刻度
#define YAW 6283
#define PITCH 6283

//摄像头中心点坐标，根据给定的窗口像素而设定，如720*480
#define CENTRE_X 360
#define CENTRE_Y 240

//抬升部分
#define G 9.8
#define RADIAN_TO_ANGLE 180.0f/(float)PI
#define MM_TO_M (float)/1000.0f
#define PitchAngleofHorizen 6339

//#define USB 1
//#define MV0 1
//#define MV1 1
//#define MV2 1
#define MV3 1
#define OLD 1

class AngleSolve
{
public:
    bool PnP_algorithm(Armor_detection &armor,std::string deck_type,std::vector<cv::Point2f> deck_2D);

    std::vector<int> Cradlehead_turn(cv::Mat transfer_matrix,int speed);
    std::vector<int> Cradlehead_set(std::vector<int> serial_scale,std::vector<int> turn_scale);

    bool Angle_solve(Armor_detection &best_armor,int speed,int gunpitch_rightnow,double &absoluteDistance);
    double solve_AngleWithGravity(float camer_SolveAngle,float speed,float absolute_Distance,float gunPitch_AngleoffsetHorizen);
    std::vector<int> angle_set(std::vector<int> &scale);

    bool AngleSolve_demo(Armor_detection &armor,std::vector<int> serial_scale,std::vector<int> &output_scale,double &absoluteDistance);

private:
    //int turn_yaw = 0,turn_pitch = 0;
    std::vector<int>turn_angle;
    cv::Mat camera_matrix; //内参矩阵
    cv::Mat distortion_matrix; //畸变矩阵
    cv::Mat Rotation_matrix = cv::Mat::zeros(3,1,CV_64FC1); //旋转矩阵
    cv::Mat transfer_matrix = cv::Mat::zeros(3,1,CV_64FC1); //平移矩阵
};


#endif //ARMOR_DETECT_ANGLESOLVE_H
